#pragma once
#include <vector>
#include <opencv2/core/core.hpp>
#include <math.h>
#include "Segment.h";
#include "SegmentHelper.h"

using namespace std;
using namespace cv;


const double DERIV_STEP = 1e-5;
const int MAX_ITER = 100;


//input: 
//return square distance between sampled Points and calculated point at segmentIndex
//residualIndex = [xc,yc,xc1,yc1,xc2,yc2,xl1,yl1,xl2,yl2]
double getResidualTwoEndsEachSegment(const Mat observedData, const Mat &fullParams,const vector<Segment*> segments, int residualIndex)
{
	int segmentIndex = 0;
	int processedParamIndex =0;
	while(true)
	{
		int numOfParamAtSegment=0;
		if(segments.at(segmentIndex)->type == SEGTYPE_LINE) numOfParamAtSegment = 4;
		else if(segments.at(segmentIndex)->type== SEGTYPE_CIRC) numOfParamAtSegment = 6;

		if(processedParamIndex + numOfParamAtSegment -1 >= residualIndex){
			break;
		}
		else{
			segmentIndex ++;//next segment
			processedParamIndex += numOfParamAtSegment;
		}
	}


	//starting point and angle of this segment
	int localParamIndex = residualIndex - processedParamIndex;
	Point2d startingPoint = getStartingPoint(fullParams, segments, 0, segmentIndex);
	double startingAngle = getStartingAngle(fullParams, segments, 0, segmentIndex);

	switch (segments.at(segmentIndex)->type)
	{
	case SEGTYPE_LINE:
		if(localParamIndex == 0)
		{
			return observedData.at<double>(residualIndex, 0) - startingPoint.x;
		}
		else if(localParamIndex == 1)
		{
			return observedData.at<double>(residualIndex, 0) - startingPoint.y;
		}
		else if(localParamIndex == 2)
		{
			double length = fullParams.at<double>(segmentIndex, 3);
			double endingPointX = startingPoint.x + length*cos(startingAngle);
			return  observedData.at<double>(residualIndex, 0) - endingPointX;
		}
		else if(localParamIndex == 3)
		{
			double length = fullParams.at<double>(segmentIndex, 3);
			double endingPointY = startingPoint.y + length*sin(startingAngle);
			return  observedData.at<double>(residualIndex, 0) - endingPointY;
		}
		break;
	case SEGTYPE_CIRC:
		double endingAngle = fullParams.at<double>(segmentIndex, 3);
		double sign = 1;//rotate direction  + non clockwise
		if(endingAngle < startingAngle) sign = -1; // clockwise

		if(localParamIndex == 0)//residual for center point
		{
			double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);
			double centerPointX = startingPoint.x - radiusOfCircle*cos(startingAngle - sign*PI/2);
			return observedData.at<double>(residualIndex, 0) - centerPointX;
		}
		else if(localParamIndex == 1)
		{
			double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);
			double centerPointY = startingPoint.y - radiusOfCircle*sin(startingAngle - sign*PI/2);
			return observedData.at<double>(residualIndex, 0) - centerPointY;
		}
		else if(localParamIndex == 2)
		{
			return observedData.at<double>(residualIndex, 0) - startingPoint.x;
		}
		else if(localParamIndex == 3)
		{
			return observedData.at<double>(residualIndex, 0) - startingPoint.y;
		}
		else if(localParamIndex == 4)
		{
			double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);

			double centerPointX = startingPoint.x - radiusOfCircle*cos(startingAngle - sign*PI/2);
			double endingPointX = centerPointX + radiusOfCircle*cos(endingAngle - sign*PI/2);

			return observedData.at<double>(residualIndex, 0) - endingPointX;
		}
		else if(localParamIndex == 5)
		{
			double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);

			double centerPointY = startingPoint.y - radiusOfCircle*sin(startingAngle - sign*PI/2);
			double endingPointY = centerPointY + radiusOfCircle*sin(endingAngle - sign*PI/2);

			return observedData.at<double>(residualIndex, 0) - endingPointY;
		}
		break;
	}


	//switch (segments.at(segmentIndex)->type)
	//{
	//case SEGTYPE_LINE:
	//	if(localParamIndex == 0)
	//	{
	//		return sqrt((observedData.at<double>(residualIndex, 0) - startingPoint.x)* (observedData.at<double>(residualIndex, 0) - startingPoint.x)
	//			+ (observedData.at<double>(residualIndex, 1) - startingPoint.y)* (observedData.at<double>(residualIndex, 1) - startingPoint.y));
	//	}
	//	else
	//	{
	//		double length = fullParams.at<double>(segmentIndex, 3);
	//		Point2d endingPoint = startingPoint + Point2d(length*cos(startingAngle), length*sin(startingAngle));
	//		return  sqrt((observedData.at<double>(residualIndex, 0) - endingPoint.x)* (observedData.at<double>(residualIndex, 0) - endingPoint.x)
	//			+ (observedData.at<double>(residualIndex, 1) - endingPoint.y)* (observedData.at<double>(residualIndex, 1) - endingPoint.y));
	//	}
	//	break;
	//case SEGTYPE_CIRC:
	//	if(localPointIndex == 0)//residual for center point
	//	{
	//		double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);
	//		Point2d centerPoint = Point2d(startingPoint.x - radiusOfCircle*cos(startingAngle - PI/2), startingPoint.y - radiusOfCircle*sin(startingAngle - PI/2));
	//		return  sqrt((observedData.at<double>(residualIndex, 0) - centerPoint.x)* (observedData.at<double>(residualIndex, 0) - centerPoint.x)
	//			+ (observedData.at<double>(residualIndex, 1) - centerPoint.y)* (observedData.at<double>(residualIndex, 1) - centerPoint.y));
	//	}
	//	else if(localPointIndex == 1)
	//	{
	//		return  sqrt((observedData.at<double>(residualIndex, 0) - startingPoint.x)* (observedData.at<double>(residualIndex, 0) - startingPoint.x)
	//			+ (observedData.at<double>(residualIndex, 1) - startingPoint.y)* (observedData.at<double>(residualIndex, 1) - startingPoint.y));
	//	}
	//	else 
	//	{
	//		double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);
	//		double endingAngle = fullParams.at<double>(segmentIndex, 3);

	//		Point2d centerPoint = Point2d(startingPoint.x - radiusOfCircle*cos(startingAngle - PI/2), startingPoint.y - radiusOfCircle*sin(startingAngle - PI/2));
	//		Point2d endingPoint = centerPoint + Point2d(radiusOfCircle*cos(endingAngle-PI/2), radiusOfCircle*sin(endingAngle-PI/2));

	//		return  sqrt((observedData.at<double>(residualIndex, 0) - endingPoint.x)* (observedData.at<double>(residualIndex, 0) - endingPoint.x)
	//			+ (observedData.at<double>(residualIndex, 1) - endingPoint.y)* (observedData.at<double>(residualIndex, 1) - endingPoint.y));
	//	}
	//	break;
	//}

	/*for(int i =0;i<residualIndex;i++)
	{
		if(segments.at(i)->type == SEGTYPE_LINE)
			processedIndex+=2;
		else if(segments.at(i)->type == SEGTYPE_CIRC)
			processedIndex+=3;
	}*/
}

double derivative(const Mat &input, const Mat &params,  const vector<Segment*> segments, int i, int paramIndex)
{
	// Assumes input is a single row matrix
	int sizeOfPrams = params.rows;
	// Returns the derivative of the nth parameter
	Mat params1 = params.clone();
	Mat params2 = params.clone();

	// Use central difference  to get derivative
	params1.at<double>(paramIndex, 0) -= DERIV_STEP;
	params2.at<double>(paramIndex, 0) += DERIV_STEP;

	Mat fullParams1 = convertToFullParams(params1, segments);
	Mat fullParams2 = convertToFullParams(params2, segments);

	//printf("params1 \n");
	//printMatrix(params1);
	//printf("params2 \n");
	//printMatrix(params2);
	//printf("fullParams1 \n");
	//printMatrix(fullParams1);
	//printf("fullParams2 \n");
	//printMatrix(fullParams2);

	double p1 = getResidualTwoEndsEachSegment(input, fullParams1, segments, i);
	double p2 = getResidualTwoEndsEachSegment(input, fullParams2, segments, i);

	double d = (p2 - p1) / (2*DERIV_STEP);
	return d;
}


void GaussNewton(const Mat &observedData, vector<Segment*> &segments)
{
	Mat params = getParamsFromSegments(segments);
    int observedDataSize = observedData.rows;
    int num_params = params.rows;

    Mat r(observedDataSize, 1, CV_64F); // residual matrix
    Mat Jr(observedDataSize, num_params, CV_64F); // Jacobian of Func()

    double last_totalSquare = 0;

    for(int i=0; i < MAX_ITER; i++) {
		printf("\n Loop i = %d  -------------------------------------------------\n", i);

        double totalSquare = 0;
		Mat fullParams = convertToFullParams(params, segments);

        for(int residualIndex=0; residualIndex < observedDataSize; residualIndex++) {
			double residual = getResidualTwoEndsEachSegment(observedData, fullParams, segments, residualIndex);
			r.at<double>(residualIndex,0) =   residual;

            totalSquare += r.at<double>(residualIndex,0)*r.at<double>(residualIndex,0);

            for(int paramIndex =0; paramIndex < num_params; paramIndex ++) 
            	Jr.at<double>(residualIndex,paramIndex) = derivative(observedData, params, segments, residualIndex, paramIndex);
        }

        //totalSquare /= m;
		printf("i= %d  totalSquare=%f  \t totalSquare - last_totalSquare = %f \n",i, totalSquare, totalSquare - last_totalSquare);
        if(fabs(totalSquare - last_totalSquare) < 1e-8) break;

        Mat delta = ((Jr.t()*Jr)).inv() * Jr.t()*r;
		
        params -= delta;
        last_totalSquare = totalSquare;
    }

	
	Mat fullParams = convertToFullParams(params, segments);
	//printf("full params\n");
	//printMatrix(fullParams);
	//saveMatrixToFile(fullParams, "D:\\M.csv");

	for(int i = 0 ;i<fullParams.rows;i++)
	{
		segments.at(i)->startingPoint.x = fullParams.at<double>(i,0);
		segments.at(i)->startingPoint.y = fullParams.at<double>(i,1);
		segments.at(i)->startingAngle = fullParams.at<double>(i,2);

		if (segments.at(i)->type == SEGTYPE_LINE)
		{
			dynamic_cast<StraightLine *>(segments.at(i))->length = fullParams.at<double>(i,3);
		}
		else if(segments.at(i)->type == SEGTYPE_CIRC)
		{
			CircleArc* arc = dynamic_cast<CircleArc *>(segments.at(i));
			arc->endingAngle =  fullParams.at<double>(i,3);
			arc->radius =  fullParams.at<double>(i,4);
		}
	}
}

void LevenbergMarquardt(const Mat &observedData, vector<Segment*> &segments, double lamda, const double &lamdaUp,const  double &lamdaDown)
{
	Mat params = getParamsFromSegments(segments);

    int observedDataSize = observedData.rows;
    int num_params = params.rows;

    Mat r(observedDataSize, 1, CV_64F); // residual matrix
    Mat Jr(observedDataSize, num_params, CV_64F); // Jacobian of Func()

    double last_totalSquare = 1000000;//infitity

    for(int i=0; i < MAX_ITER; i++) {
		printf("\n Loop i = %d  -------------------------------------------------\n", i);

        double totalSquare = 0;
		Mat fullParams = convertToFullParams(params, segments);

        for(int residualIndex=0; residualIndex < observedDataSize; residualIndex++) {
			double residual = getResidualTwoEndsEachSegment(observedData, fullParams, segments, residualIndex);
			r.at<double>(residualIndex,0) =   residual;
            totalSquare += r.at<double>(residualIndex,0)*r.at<double>(residualIndex,0);
            for(int paramIndex =0; paramIndex < num_params; paramIndex ++) 
            	Jr.at<double>(residualIndex,paramIndex) = derivative(observedData, params, segments, residualIndex, paramIndex);
        }

		printf("i= %d  totalSquare=%f  \t last_totalSquare -totalSquare = %f \n",i, totalSquare, last_totalSquare -totalSquare);
		if(totalSquare < last_totalSquare)
		{
			Mat identity = Mat::eye(num_params, num_params, CV_64F);
			Mat delta = ((Jr.t()*Jr  + lamda*identity)).inv() * Jr.t()*r;
			params = params - delta;
			lamda = lamda/lamdaDown;

			if(fabs(totalSquare - last_totalSquare) < 1e-6) break;
			last_totalSquare = totalSquare;
		}
		else
		{
			lamda = lamda*lamdaUp;
			//last_totalSquare = totalSquare;
			continue;
		}
    }

	
	Mat fullParams = convertToFullParams(params, segments);
	//printf("full params\n");
	//printMatrix(fullParams);
	//saveMatrixToFile(fullParams, "D:\\M.csv");

	for(int i = 0 ;i<fullParams.rows;i++)
	{
		segments.at(i)->startingPoint.x = fullParams.at<double>(i,0);
		segments.at(i)->startingPoint.y = fullParams.at<double>(i,1);
		segments.at(i)->startingAngle = fullParams.at<double>(i,2);

		if (segments.at(i)->type == SEGTYPE_LINE)
		{
			dynamic_cast<StraightLine *>(segments.at(i))->length = fullParams.at<double>(i,3);
		}
		else if(segments.at(i)->type == SEGTYPE_CIRC)
		{
			CircleArc* arc = dynamic_cast<CircleArc *>(segments.at(i));
			arc->endingAngle =  fullParams.at<double>(i,3);
			arc->radius =  fullParams.at<double>(i,4);
		}
	}
}


